# -*- coding: utf-8 -*-
"""
Created on Fri Dec 24 17:12:09 2021

@author: wulong
"""
import numpy as np
from casadi import *
from Slow_formulate_Opt import *
from Obtain_results import *

def S_MPC(initial_state, state_bnds_lo, state_bnds_up, 
          medium_state_bnds_lo, medium_state_bnds_up, 
          fast_state_bnds_lo, fast_state_bnds_up, 
          fast_state_slack_lo, fast_state_slack_up, 
          input_guess, input_bnds_lo, input_bnds_up, 
          medium_input_bnds_lo, medium_input_bnds_up, 
          fast_input_bnds_lo, fast_input_bnds_up, 
          input_bin_bnds,
          distb_bnds,
          output_guess, 
          yspe_bnds_lo, yspe_bnds_up,
          output_setpnts, long_state_setpnts, 
          alpha, alpha_slack, pred_horzn, i, i_l_s):
    
    initial_state_new = np.append(np.insert(initial_state[18:21], 0, 
                                            initial_state[16]), initial_state[22])
    state_guess_new = initial_state_new
    medium_state_guess_new = np.append(initial_state[3:9], 
                                       np.array([initial_state[11], initial_state[14], 
                                                 initial_state[21]]))
    fast_state_guess_new = np.delete(initial_state, 
                                     np.array((3,4,5,6,7,8,11,14,16,18,19,20,21,22)))
    input_guess_new = np.insert(input_guess[4:], 0, input_guess[2])
    medium_input_guess_new = input_guess[1]
    fast_input_guess_new = np.array([input_guess[0], input_guess[3]])
    distb_bnds_new = distb_bnds[i:i+pred_horzn, :]
    yspe_bnds_lo_new = yspe_bnds_lo[i:i+pred_horzn]
    yspe_bnds_up_new = yspe_bnds_up[i:i+pred_horzn]
    yspe_guess_new = 0.5*(yspe_bnds_lo_new[0] + yspe_bnds_up_new[0])
    output_setpnts_new = output_setpnts[i:i+pred_horzn]
    long_state_setpnts_new = long_state_setpnts[i_l_s:i_l_s+pred_horzn,:]
    
    f_opt = formulate_opt_s(initial_state_new, state_guess_new, state_bnds_lo, state_bnds_up, 
                    medium_state_guess_new, medium_state_bnds_lo, medium_state_bnds_up, 
                    fast_state_guess_new, fast_state_bnds_lo, fast_state_bnds_up, 
                    fast_state_slack_lo, fast_state_slack_up, 
                    input_guess_new, input_bnds_lo, input_bnds_up, 
                    medium_input_guess_new, medium_input_bnds_lo, medium_input_bnds_up, 
                    fast_input_guess_new, fast_input_bnds_lo, fast_input_bnds_up, 
                    input_bin_bnds, 
                    distb_bnds_new, 
                    output_guess, 
                    yspe_guess_new, yspe_bnds_lo_new, yspe_bnds_up_new,
                    output_setpnts_new, long_state_setpnts_new, 
                    alpha, alpha_slack, pred_horzn)
    
    s_opt = solve_opt_s(*f_opt)
    
    r_opt = get_results_s(s_opt, pred_horzn)
    
    return r_opt
